/*
 * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <crypto_mod.h>
#include <debug.h>
#include <mbedtls_common.h>
#include <stddef.h>
#include <string.h>

/* mbed TLS headers */
#include <mbedtls/md.h>
#include <mbedtls/memory_buffer_alloc.h>
#include <mbedtls/oid.h>
#include <mbedtls/platform.h>
#include <sm2.h>
#include <sm3.h>
#include <smx_common.h>

#define LIB_NAME		"mbed TLS"
/*
 * AlgorithmIdentifier  ::=  SEQUENCE  {
 *     algorithm               OBJECT IDENTIFIER,
 *     parameters              ANY DEFINED BY algorithm OPTIONAL
 * }
 *
 * SubjectPublicKeyInfo  ::=  SEQUENCE  {
 *     algorithm            AlgorithmIdentifier,
 *     subjectPublicKey     BIT STRING
 * }
 *
 * DigestInfo ::= SEQUENCE {
 *     digestAlgorithm AlgorithmIdentifier,
 *     digest OCTET STRING
 * }
 */

/*
 * Initialize the library and export the descriptor
 */
static void init(void)
{
	/* Initialize mbed TLS */
	mbedtls_init();
}

/*
 * Verify a signature.
 *
 * Parameters are passed using the DER encoding format following the ASN.1
 * structures detailed above.
 */
static int verify_signature(void *data_ptr, unsigned int data_len,
			    void *sig_ptr, unsigned int sig_len,
			    void *sig_alg, unsigned int sig_alg_len,
			    void *pk_ptr, unsigned int pk_len)
{
	mbedtls_asn1_buf sig_oid, sig_params;
	mbedtls_asn1_buf signature;
	//mbedtls_md_type_t md_alg;
	//mbedtls_pk_type_t pk_alg;
	mbedtls_pk_context pk = {0};
	int rc,i;
	void *sig_opts = NULL;
	//const mbedtls_md_info_t *md_info;
	unsigned char *p, *end;
	//unsigned char hash[MBEDTLS_MD_MAX_SIZE];
	unsigned char sm2_sign_alg[] = { 0x2A, 0x81, 0x1C, 0xCF, 0x55, 0x01, 0x83, 0x75 };
	uint8_t id[16] = {0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38};
	uint8_t pubkey[65];
	uint8_t sign[64];
	uint8_t Z[32], E[32];
	
	/* Get pointers to signature OID and parameters */
	p = (unsigned char *)sig_alg;
	end = (unsigned char *)(p + sig_alg_len);
	rc = mbedtls_asn1_get_alg(&p, end, &sig_oid, &sig_params);
	if (rc != 0) {
		return CRYPTO_ERR_SIGNATURE;
	}

	/*auth sign alg is sm2sign-with-sm3*/
	rc = memcmp(sig_oid.p, sm2_sign_alg, sig_oid.len);
	if (rc != 0) {
		return CRYPTO_ERR_SIGNATURE;
	}

#if 0
	/* Get the actual signature algorithm (MD + PK) */
	rc = mbedtls_x509_get_sig_alg(&sig_oid, &sig_params, &md_alg, &pk_alg, &sig_opts);
	if (rc != 0) {
		return CRYPTO_ERR_SIGNATURE;
	}
	
#endif
	/* Parse the public key */
	mbedtls_pk_init(&pk);
	p = (unsigned char *)pk_ptr;
	end = (unsigned char *)(p + pk_len);
	rc = mbedtls_pk_parse_subpubkey(&p, end, &pk);
	if (rc != 0) {
		rc = CRYPTO_ERR_SIGNATURE;
		goto end2;
	}
	
	/*storage in pubkey*/
	for(i = 0; i <65; i++, p++) {
		pubkey[i] = *p;
	}
	
	/* Get the signature (bitstring) */
	p = (unsigned char *)sig_ptr;
	end = (unsigned char *)(p + sig_len);
	signature.tag = *p;
	rc = mbedtls_asn1_get_bitstring_null(&p, end, &signature.len);
	if (rc != 0) {
		rc = CRYPTO_ERR_SIGNATURE;
		goto end1;
	}
	
	signature.p = p;
	rc = mbedtls_asn1_get_tag( &p, end, &signature.len, MBEDTLS_ASN1_CONSTRUCTED | MBEDTLS_ASN1_SEQUENCE);
	if (rc != 0) {
		rc = CRYPTO_ERR_SIGNATURE;
		goto end1;
	}
	
	rc = mbedtls_asn1_get_tag( &p, end, &signature.len, MBEDTLS_ASN1_INTEGER);
	if (rc != 0) {
		rc = CRYPTO_ERR_SIGNATURE;
		goto end1;
	}
	
	if((*p == 0) && (signature.len == 0x21)) {
		p++;
		for(i = 0; i < 32; i++, p++) {
			sign[i] = *p;
		}
	
		rc = mbedtls_asn1_get_tag( &p, end, &signature.len, MBEDTLS_ASN1_INTEGER);	
		if (rc != 0) {
				rc = CRYPTO_ERR_SIGNATURE;
				goto end1;
		}
		
		if((*p == 0) && (signature.len == 0x21)) {
			p++;
			for(i = 0; i < 32; i++, p++) {
				sign[i+32] = *p;
			}	
		} else if( signature.len == 0x20) {
			for(i = 0; i < 32; i++, p++) {
				sign[i+32] = *p;
			}
		} else {	
			return CRYPTO_ERR_SIGNATURE;
		}
		
	}else if(signature.len == 0x20) {

		for(i = 0; i < 32; i++, p++) {
			sign[i] = *p;
		}
		
		rc = mbedtls_asn1_get_tag( &p, end, &signature.len, MBEDTLS_ASN1_INTEGER);	
		if (rc != 0) {
				rc = CRYPTO_ERR_SIGNATURE;
				goto end1;
		}

		if((*p == 0) && (signature.len == 0x21)) {
			p++;
			for(i = 0; i < 32; i++, p++) {	
				sign[i+32] = *p;
			}
		} else if( signature.len == 0x20) {
			for(i = 0; i < 32; i++, p++) {
				sign[i+32] = *p;
			}
		} else {
		
			return CRYPTO_ERR_SIGNATURE;
		}
	}
	
#ifdef SM_DEBUG	
	/*printf pubkey*/
	print_buf_U8(pubkey, 65, "pubkey");
	/*printf signature*/
	print_buf_U8(sign, 64, "pubkey");
#endif

	/* Calculate the hash of the data */

	/*get Z value */
	rc = sm2_getZ(id, 16, pubkey, Z);
	if(rc != 0) {
		return CRYPTO_ERR_HASH;
	}

	/*get E value */
	p = (unsigned char *)data_ptr;
	rc = sm2_getE(p, data_len, Z, E);
	if(rc != 0) {
		return CRYPTO_ERR_HASH;
	}
	
	/* verify */
	rc = sm2_verify(E, pubkey, sign);
	if (rc != 0) {
		rc = CRYPTO_ERR_SIGNATURE;
		goto end1;
	}


#if 0	
	/* Calculate the hash of the data */
	md_info = mbedtls_md_info_from_type(md_alg);
	if (md_info == NULL) {
		rc = CRYPTO_ERR_SIGNATURE;
		goto end1;
	}
	p = (unsigned char *)data_ptr;
	rc = mbedtls_md(md_info, p, data_len, hash);
	if (rc != 0) {
		rc = CRYPTO_ERR_SIGNATURE;
		goto end1;
	}

	/* Verify the signature */
	rc = mbedtls_pk_verify_ext(pk_alg, sig_opts, &pk, md_alg, hash,
			mbedtls_md_get_size(md_info),
			signature.p, signature.len);
	if (rc != 0) {
		rc = CRYPTO_ERR_SIGNATURE;
		goto end1;
	}

	/* Signature verification success */
#endif
	rc = CRYPTO_SUCCESS;

end1:
	mbedtls_pk_free(&pk);
end2:
	mbedtls_free(sig_opts);
	return rc;
}

/*
 * Match a hash
 *
 * Digest info is passed in DER format following the ASN.1 structure detailed
 * above.
 */
static int verify_hash(void *data_ptr, unsigned int data_len,
		       void *digest_info_ptr, unsigned int digest_info_len)
{
	mbedtls_asn1_buf hash_oid, params;
//	mbedtls_md_type_t md_alg;
//	const mbedtls_md_info_t *md_info;
	unsigned char *p, *end, *hash;
	unsigned char data_hash[32];
	unsigned char sm3_digst[8] = {0x2A,0x81,0x1C,0xCF,0x55,0x01,0x83,0x11};
	hash_context_t ctx[1];
	size_t len;
	int rc;
	
	printf("verify_hash enter\n");
	/* Digest info should be an MBEDTLS_ASN1_SEQUENCE */
	p = (unsigned char *)digest_info_ptr;
	end = p + digest_info_len;
	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_CONSTRUCTED |
				  MBEDTLS_ASN1_SEQUENCE);
	
	if (rc != 0) {
		printf("verify_hash  digest_info_ptr enter\n");
		return CRYPTO_ERR_HASH;
	}
#ifdef SM_DEBUG
		print_buf_U8(p, len, "MBEDTLS_ASN1_SEQUENCE");
#endif

	/* Get the hash algorithm */
	rc = mbedtls_asn1_get_alg(&p, end, &hash_oid, &params);
	if (rc != 0) {
		printf("mbedtls_asn1_get_alg(&p, end, &hash_oid, &params);\n");
		return CRYPTO_ERR_HASH;
	}
#ifdef SM_DEBUG
			print_buf_U8(p, len, " hash algorithm");
#endif

	rc = memcmp(hash_oid.p, sm3_digst, hash_oid.len);
	if (rc != 0) {
		return CRYPTO_ERR_HASH;
	}
	printf("memcmp(hash_oid.p, sm3_digst, hash_oid.len)\n");		
#if 0	
	rc = mbedtls_oid_get_md_alg(&hash_oid, &md_alg);
	if (rc != 0) {
		return CRYPTO_ERR_HASH;
	}

	md_info = mbedtls_md_info_from_type(md_alg);
	if (md_info == NULL) {
		return CRYPTO_ERR_HASH;
	}
#endif
	/* Hash should be octet string type */
	rc = mbedtls_asn1_get_tag(&p, end, &len, MBEDTLS_ASN1_OCTET_STRING);
	if (rc != 0) {
		printf("error:Hash should be octet string type\n");
		print_buf_U8(p, len, "1hash");
		return CRYPTO_ERR_HASH;
	}
	
	hash = p;
	
#ifdef SM_DEBUG
	print_buf_U8(p, len, "hash");
#endif

#if 0
	/* Length of hash must match the algorithm's size */
	if (len != mbedtls_md_get_size(md_info)) {
		return CRYPTO_ERR_HASH;
	}
#endif
	/* Calculate the hash of the data */
	p = (unsigned char *)data_ptr;
#ifdef SM_DEBUG
		print_buf_U8(p+data_len-128, 128, "data buf");
#endif

	rc = sm3_init(ctx);
	if(rc) {
		printf("error:sm3_init\n");
		return rc;
	}
	
	rc = sm3_process(ctx, p, data_len);
	if(rc) {
		printf("error:sm3_process\n");
		return rc;
	}
	
	rc = sm3_done(ctx, data_hash);
	if(rc) {
		printf("error:sm3_done\n");
		return rc;
	}
#ifdef SM_DEBUG
		print_buf_U8(data_hash, sizeof(data_hash), "data_hash");
#endif

#if 0	
	rc = mbedtls_md(md_info, p, data_len, data_hash);
	if (rc != 0) {
		return CRYPTO_ERR_HASH;
	}
#endif

	/* Compare values */
	rc = memcmp(data_hash, hash, 32);
	if (rc != 0) {
		printf("error: memcmp(data_hash, hash, 32);\n");
		return CRYPTO_ERR_HASH;
	}

	return CRYPTO_SUCCESS;
}

/*
 * Register crypto library descriptor
 */
REGISTER_CRYPTO_LIB(LIB_NAME, init, verify_signature, verify_hash);
